# -*- coding: utf-8 -*-
"""
Spyder Editor

This temporary script file is located here:
C:\Documents and Settings\Andrzej\.spyder2\.temp.py
"""

import os
import re 
import numpy as np
from PyDaq.Hardware.Interface import SerialInterface, TelnetInterface, dbg
import matplotlib.pyplot as plt
import time

PROMPT_FOUND     = 1
PROMPT_NOT_FOUND = 0
PORT_OPEN        = 1
PORT_CLOSED      = 0
PORT_ERROR       = -1

TIMEOUT          = 2


#os.chdir('E:\home\data')
#print os.getcwd()
#file_list = os.listdir(os.getcwd())
#print file_list

def runmo(mode=4, speed=4000, delay=2000):
    
    M = Mo()
    M.open()
    cmd = "set 1 %d" % speed
    M.send(cmd)
    time.sleep(1)
    response = M.query(cmd="run %d %d" % (mode, delay), expected_text="</run>", tag="run")
    M.close()
    print response
    print "Run finished"
    
    if mode == 9:
        filename = 'modata.txt'
        fid = open(filename,'w');fid.write(response);fid.close()
        a = np.genfromtxt(filename,int)
        
        plt.subplot(411)
        plt.contourf(a[:,0:6].transpose(),255)
        plt.xlim(128,a.shape[0])
        
        plt.subplot(412)
        plt.plot(a[:,8])
        plt.plot(a[:,9])
        plt.plot(a[:,10])
        plt.xlim(128,a.shape[0])
        
        plt.subplot(413)
        plt.plot(a[:,12])
        plt.plot(a[:,13])
        plt.xlim(128,a.shape[0])
        
        plt.subplot(414)
        plt.plot(a[:,14])
        plt.plot(a[:,15])
        plt.plot(a[:,16])
        plt.xlim(128,a.shape[0])
        plt.show()
        return a
    elif mode == 4:
        var = np.array(map(int,response.split()))
        acquiering = var == 0
        tracking   = var == 1
        sharp_l    = var == 2
        sharp_r    = var == 3
        line_break = var == 4
        crossing   = var == 5
        all_stop   = var == 6
        acute_l    = var == 7
        acute_r    = var == 8
         
        n = np.arange(len(var))
        plt.plot(n[acquiering],var[acquiering],'-g')
        plt.plot(n[tracking],var[tracking],'-g')
        plt.plot(n[sharp_l],var[sharp_l],'-b')
        plt.plot(n[sharp_r],var[sharp_r],'-k')
        plt.plot(n[line_break],var[line_break],'-r')
        plt.show()         
    else:
        var = map(int,response.split())     
        plt.plot(var,'o')
        plt.show()
    return var
    
    
    
def PlotError(error_vector, dt):
    ev = np.array(error_vector)     
    t  = np.linspace(0,dt*ev.size,ev.size)
    title_text = 'Run time %.1f sec, dt %.2f msec' % (dt*ev.size, dt/1e-3) 
    plt.subplot(211)
    plt.plot(t,ev,'-+')
    plt.xlabel("Time ")
    plt.ylabel("Error")
    plt.ylim(-250,250)
    plt.grid()
    plt.title(title_text)
          
    plt.subplot(212)
    evf = np.fft.fft(ev)
    f   = np.fft.fftfreq(ev.size,dt)
    plt.plot(f,np.absolute(evf))
    plt.xlim(0,30)
    plt.xlabel("Frequency [Hz]")
    plt.ylabel("Error PSD")
    plt.show()

class Mo:
    """
    The Mo interface object - Mini Ore32 Line Follower
    """
       
    debug = 0;
    Interface = None    

    def __init__(self, host='COM', port=14): 
        if host == 'COM':        
            self.Interface = SerialInterface(port=port);
            self.Interface.Ser.baudrate=115200
        else:
            self.Interface = TelnetInterface(host,port=2000);
        dbg(self,0, "Arduino(host=%s, port=%d)" %(host, port))
        self.open()

    def  __unicode__(self):
        msg = 'Mo'
        return msg
        
    def __del__(self):
        dbg(self, 2, "def __del__(self):")
        self.close()

    def open(self):
        dbg(self, 2, "def open(self):")
        self.Interface.open()

    def close(self):
        dbg(self, 2, "def close(self):")
        self.Interface.close()        
        return self.Interface.isOpen()
    
    def send(self, cmd):
        msg = 'def send(self, cmd="%s"):' % cmd
        dbg(self, 2, msg)
        self.Interface.send(cmd)
    
    def query(self, cmd, expected_text='', tag=''):
        dbg(self, 1, "def query(self,cmd=%s):" % cmd)
        if tag:
            expected_text = '</' + tag + '>'
        if self.prompt():
            self.send(cmd)
            out   = self.read(expected_text)
            if tag:
                pattern = re.compile(r'(?:<{0}>)(.*)(?:</{0}>)'.format(tag), re.DOTALL)
                temp = pattern.findall(out)
                if len(temp)>0:
                    out = temp[0]
                else:
                    out = ''
            return out           
        else:
            dbg(self,1, "Empty query for: %s" % cmd)
            return ''
                   
    def read(self,expected_text=''):
        dbg(self, 2, "def read(self):")
        response = self.Interface.read(expected_text)
        dbg(self, 3, "Response: " + response)
        return response
    
    def prompt(self,attempt=1):
        
        max_attempt = 3
        self.Interface.send('q')
        prompt = self.Interface.GetPrompt()        
        if prompt == PROMPT_FOUND:
            return True
        elif prompt == PORT_ERROR:
            self.Interface.close()
            time.sleep(0.5)
            self.Interface.open()
            if attempt > max_attempt:
                return False
            else:
                attempt+=1
                self.prompt(attempt)
        else:
            if attempt > max_attempt:
                return False
            else:
                attempt+=1
                self.prompt(attempt)
        
    def Run(self, mode=2,delay=1000):        
        to = time.clock()
        response = self.query(cmd="run %d %d" % (mode, delay), expected_text="</run>", tag="run")        
        tn = time.clock()
        error_vector = map(int,response.split())       
        self.close()
        dt = tn-to
        n  = len(error_vector)
        print "Elapsed time %.2f, num of pts: %d, loop period = %e"  % (dt, n, dt/n)
        return error_vector
    
    def Cal(self, delay=3, num_of_pts=200):
        cmd = "cal %d %d" % (delay, num_of_pts)
        response = self.query(cmd,expected_text="</cal>",tag='cal')        
        temp = response.split('\r\n')
        temp = temp[1:-1]
        m = []
        for line in temp:
            tmp = line.split()
            m.append(map(int,tmp))        
        a = np.array(m)
        
        plt.subplot(3,2,1)
        for i in range(6):
            plt.plot(a[:,i])
        plt.ylabel('ADC [raw]')        
        
        plt.subplot(3,2,2)
        for i in range(2):
            plt.plot(a[:,i+6])
                    
        plt.subplot(3,2,3)
        for i in range(6):
            plt.plot(a[:,i+8])                    
        plt.ylabel('ADC [norm]') 
        plt.subplot(3,2,4)
        for i in range(2):
            plt.plot(a[:,i+14])   
        plt.subplot(3,2,5)
        plt.plot(a[:,16])
        plt.plot(a[:,17])
        plt.ylabel('Error')
        plt.subplot(3,2,6)
        plt.plot(a[:,14]-a[:,15])
        plt.show()
        
        self.close()
        return a
    
    def set(self, setting, val):
        cmd = "set %d %d" %(setting, val)
        self.send(cmd)
        
    def get(self):
        str = self.query("?", "\r\n\x1b[0mcmd>")
        expr = re.compile(r'(?:PWM MAX\s+)(\d{1,4})')
        
        
    
    def PwmCal(self, pwm, delay):
        cmd = "test %d %d" % (pwm, delay)
        to = time.clock()
        response = self.query(cmd,tag='run')
        self.close()
        tn = time.clock()
        print tn - to
        error_vector = map(int,response.split())
        v = np.array(error_vector)
        v = abs(v)
        ix = v > 20
        v[ix] = 1        
        v[np.logical_not(ix)] = 0
        dv = np.diff(v)
        dv = np.nonzero(dv)
        print dv
        print dv[0][2] - dv[0][1]
        plt.plot(v)
        plt.show()
        
        return dv
